
#include "can_simple.hpp"
#include <odrive_main.h>

#include <cstring>

static constexpr uint8_t NUM_NODE_ID_BITS = 6;
static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS;
static uint32_t emergency_stop = 0x00;
void CANSimple::handle_can_message(can_Message_t& msg) {
    // This functional way of handling the messages is neat and is much cleaner from
    // a data security point of view, but it will require some tweaking to fix the syntax.
    //
    // auto func = callback_map.find(msg.id);
    // if(func != callback_map.end()){
    //     func->second(msg);
    // }

    //     Frame
    // nodeID | CMD
    // 6 bits | 5 bits
    uint32_t nodeID = get_node_id(msg.id);
    uint32_t cmd = get_cmd_id(msg.id);
	uint32_t cmd_Ext =msg.id>>18;
	Axis* axis = nullptr;  
    bool validAxis = false;
    for (uint8_t i = 0; i < AXIS_COUNT; i++) {
       if ((axes[i]->config_.can_node_id == nodeID) && (axes[i]->config_.can_node_id_extended == msg.isExt)) {
            axis = axes[i];
            if (!validAxis) {
                validAxis = true;
            } else {
                // Duplicate can IDs, don't assign to any axis
                odCAN->set_error(ODriveCAN::ERROR_DUPLICATE_CAN_IDS);
                validAxis = false;
                break;
            }
        }
    }

    if(cmd_Ext == 0x44){
        emergency_stop = msg.buf[3]; 
        if(emergency_stop){
            axes[0]->controller_.input_vel_can_ = 0;
            axes[1]->controller_.input_vel_can_ = 0;
            axes[0]->controller_.input_vel_ = 0;
            axes[1]->controller_.input_vel_ = 0;
            if(axes[0]->config_.enable_debug==false)
            {
                axes[0]->requested_state_=static_cast<Axis::AxisState>(1);
                axes[1]->requested_state_=static_cast<Axis::AxisState>(1);
            }
        }
    }
#if 0
    if(msg.id == 0x0000002d){
        axes[0]->encoder_.shadow_count_40ms_ = msg.id;
	}
    else if(msg.id == 0x0000002e){
        axes[0]->encoder_.shadow_count_40ms_ = msg.id;
	}else{
        axes[0]->encoder_.shadow_count_40ms_ = msg.id;
	}
#endif
    if (validAxis) {
        axis->watchdog_feed();
        switch (cmd) {
            //case MSG_CO_NMT_CTRL:
            //    break;
            //case MSG_CO_HEARTBEAT_CMD:
            //    break;
            case MSG_ODRIVE_HEARTBEAT:
                // We don't currently do anything to respond to ODrive heartbeat messages
                send_heartbeat(axis);
                break;
            //case MSG_ODRIVE_ESTOP:
            //    estop_callback(axis, msg);
            //    break;
            //case MSG_GET_MOTOR_ERROR:
            //    get_motor_error_callback(axis, msg);
            //    break;
            //case MSG_GET_ENCODER_ERROR:
            //    get_encoder_error_callback(axis, msg);
            //    break;
            //case MSG_GET_SENSORLESS_ERROR:
            //    get_sensorless_error_callback(axis, msg);
            //    break;
            //case MSG_SET_AXIS_NODE_ID:
            //    set_axis_nodeid_callback(axis, msg);
            //    break;
            case MSG_SET_AXIS_REQUESTED_STATE:
                set_axis_requested_state_callback(axis, msg);
                break;
            //case MSG_SET_AXIS_STARTUP_CONFIG:
            //    set_axis_startup_config_callback(axis, msg);
            //    break;
            case MSG_GET_ENCODER_ESTIMATES:
                get_encoder_estimates_callback(axis, msg);
                break;
            //case MSG_GET_ENCODER_COUNT:
            //    get_encoder_count_callback(axis, msg);
            //    break;
            //case MSG_SET_INPUT_POS:
            //    set_input_pos_callback(axis, msg);
            //    break;
            case MSG_SET_INPUT_VEL:
                set_input_vel_callback(axis, msg);
                break;
            //case MSG_SET_INPUT_TORQUE:
            //    set_input_torque_callback(axis, msg);
            //    break;
            //case MSG_SET_CONTROLLER_MODES:
            //    set_controller_modes_callback(axis, msg);
            //    break;
            //case MSG_SET_VEL_LIMIT:
            //    set_vel_limit_callback(axis, msg);
            //    break;
            //case MSG_START_ANTICOGGING:
            //    start_anticogging_callback(axis, msg);
            //    break;
            //case MSG_SET_TRAJ_INERTIA:
            //    set_traj_inertia_callback(axis, msg);
            //    break;
            //case MSG_SET_TRAJ_ACCEL_LIMITS:
            //    set_traj_accel_limits_callback(axis, msg);
            //    break;
            //case MSG_SET_TRAJ_VEL_LIMIT:
            //    set_traj_vel_limit_callback(axis, msg);
            //    break;
            //case MSG_GET_IQ:
            //    get_iq_callback(axis, msg);
            //    break;
            //case MSG_GET_SENSORLESS_ESTIMATES:
            //    get_sensorless_estimates_callback(axis, msg);
            //    break;
            case MSG_RESET_ODRIVE:
                NVIC_SystemReset();
                break;
            //case MSG_GET_VBUS_VOLTAGE:
            //    get_vbus_voltage_callback(axis, msg);
            //    break;
            case MSG_CLEAR_ERRORS:
                clear_errors_callback(axis, msg);
                break;
			case MSG_DEBUG:
			    send_debug(axis, msg);
				break;
			//case MSG_SET_CURRENT_STATE:
			//    set_axis_current_state_callback(axis, msg);
			//	break;
			case MSG_GET_TWO_ENCODER_ESTIMATES:
			    get_two_encoder_estimates_callback(axis, msg);
				break;
			//case MSG_SET_TEST_STATE:
			//    set_axis_test_state_callback(axis, msg);
			//	break;
			case MSG_GET_TWO_ENCODER_VEL:
			    get_two_encoder_shadow_count_20ms_callback(axis, msg);
				break;
			case MSG_GET_ERROR_INFO:
			    get_drive_error_callback(axis, msg);
				break;
            default:
                break;
        }
    }
}

void CANSimple::nmt_callback(Axis* axis, can_Message_t& msg) {
    ;// Not implemented
}

void CANSimple::estop_callback(Axis* axis, can_Message_t& msg) {
    axis->error_ = Axis::ERROR_ESTOP_REQUESTED;
}

void CANSimple::get_motor_error_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        can_Message_t txmsg;
        txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
        txmsg.id += MSG_GET_MOTOR_ERROR;  // heartbeat ID
        txmsg.isExt = axis->config_.can_node_id_extended;
        txmsg.len = 8;

        txmsg.buf[0] = axis->motor_.error_;
        txmsg.buf[1] = axis->motor_.error_ >> 8;
        txmsg.buf[2] = axis->motor_.error_ >> 16;
        txmsg.buf[3] = axis->motor_.error_ >> 24;

        odCAN->write(txmsg);
    }
}

void CANSimple::get_encoder_error_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        ;
    }
}

void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        ;
    }
}

void CANSimple::set_axis_nodeid_callback(Axis* axis, can_Message_t& msg) {
    axis->config_.can_node_id = can_getSignal<uint32_t>(msg, 0, 32, true);
}

void CANSimple::set_axis_requested_state_callback(Axis* axis, can_Message_t& msg) {
    //axis->requested_state_ = static_cast<Axis::AxisState>(can_getSignal<int32_t>(msg, 0, 16, true));
    axes[0]->requested_state_ = static_cast<Axis::AxisState>(can_getSignal<int32_t>(msg, 0, 16, true));
	axes[1]->requested_state_ = axes[0]->requested_state_;
}

void CANSimple::set_axis_current_state_callback(Axis* axis, can_Message_t& msg) {
    axis->current_state_ = static_cast<Axis::AxisState>(can_getSignal<int32_t>(msg, 0, 16, true));
}
 
void CANSimple::set_axis_startup_config_callback(Axis* axis, can_Message_t& msg) {
    // Not Implemented
}

void CANSimple::get_encoder_estimates_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        can_Message_t txmsg;
        txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
        txmsg.id += MSG_GET_ENCODER_ESTIMATES;  // heartbeat ID
        txmsg.isExt = axis->config_.can_node_id_extended;
        txmsg.len = 8;

        int32_t m0_shadow_count = 0 , m1_shadow_count = 0;

        static_assert(sizeof m0_shadow_count == sizeof axes[0]->encoder_.shadow_count_);
        std::memcpy(&m0_shadow_count, &axes[0]->encoder_.shadow_count_, sizeof m0_shadow_count);
		
        txmsg.buf[0] = m0_shadow_count >> 0;
        txmsg.buf[1] = m0_shadow_count >> 8;
        txmsg.buf[2] = m0_shadow_count >> 16;
        txmsg.buf[3] = m0_shadow_count >> 24;

        static_assert(sizeof m1_shadow_count == sizeof axes[1]->encoder_.shadow_count_);
        std::memcpy(&m1_shadow_count, &axes[1]->encoder_.shadow_count_, sizeof m1_shadow_count);
		
        txmsg.buf[4] = m1_shadow_count >> 0;
        txmsg.buf[5] = m1_shadow_count >> 8;
        txmsg.buf[6] = m1_shadow_count >> 16;
        txmsg.buf[7] = m1_shadow_count >> 24;

        odCAN->write(txmsg);
    }
}
void CANSimple::get_two_encoder_estimates_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        can_Message_t txmsg;
        txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
        txmsg.id += MSG_GET_TWO_ENCODER_ESTIMATES;  
        txmsg.isExt = axis->config_.can_node_id_extended;
        txmsg.len = 8;
		
        int32_t m0_floatBytes = (int32_t)(-axes[0]->encoder_.shadow_count_);  
        txmsg.buf[0] = m0_floatBytes;
        txmsg.buf[1] = m0_floatBytes >> 8;
        txmsg.buf[2] = m0_floatBytes >> 16;
        txmsg.buf[3] = m0_floatBytes >> 24;

        int32_t m1_floatBytes = (int32_t)(-axes[1]->encoder_.shadow_count_); 
        txmsg.buf[4] = m1_floatBytes;
        txmsg.buf[5] = m1_floatBytes >> 8;
        txmsg.buf[6] = m1_floatBytes >> 16;
        txmsg.buf[7] = m1_floatBytes >> 24;

        odCAN->write(txmsg);
    }
}
void CANSimple::get_two_encoder_shadow_count_20ms_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        can_Message_t txmsg;
        txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
        txmsg.id += MSG_GET_TWO_ENCODER_VEL;  
        txmsg.isExt = axis->config_.can_node_id_extended;
        txmsg.len = 8;
		
        int32_t m0_floatBytes = (int32_t)(axes[0]->encoder_.shadow_count_20ms_); 
        txmsg.buf[0] = m0_floatBytes;
        txmsg.buf[1] = m0_floatBytes >> 8;
        txmsg.buf[2] = m0_floatBytes >> 16;
        txmsg.buf[3] = m0_floatBytes >> 24;

        int32_t m1_floatBytes = (int32_t)(axes[1]->encoder_.shadow_count_20ms_); 
        txmsg.buf[4] = m1_floatBytes;
        txmsg.buf[5] = m1_floatBytes >> 8;
        txmsg.buf[6] = m1_floatBytes >> 16;
        txmsg.buf[7] = m1_floatBytes >> 24;

        odCAN->write(txmsg);
    }
}

void CANSimple::get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        ;
    }
}

void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        can_Message_t txmsg;
        txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
        txmsg.id += MSG_GET_ENCODER_COUNT;
        txmsg.isExt = axis->config_.can_node_id_extended;
        txmsg.len = 8;

        txmsg.buf[0] = axis->encoder_.shadow_count_;
        txmsg.buf[1] = axis->encoder_.shadow_count_ >> 8;
        txmsg.buf[2] = axis->encoder_.shadow_count_ >> 16;
        txmsg.buf[3] = axis->encoder_.shadow_count_ >> 24;

        txmsg.buf[4] = axis->encoder_.count_in_cpr_;
        txmsg.buf[5] = axis->encoder_.count_in_cpr_ >> 8;
        txmsg.buf[6] = axis->encoder_.count_in_cpr_ >> 16;
        txmsg.buf[7] = axis->encoder_.count_in_cpr_ >> 24;

        odCAN->write(txmsg);
    }
}

void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) {
    ;
}

void CANSimple::set_input_vel_callback(Axis* axis, can_Message_t& msg) {

    if(msg.buf[0] == 0x00) //�����ٶ�
    {   
        /* if(emergency_stop==0x01){
            memset(msg.buf,0x00,8);
        } */
        axes[0]->controller_.input_vel_can_ = -can_getSignal<int16_t>(msg, 48, 16, false);  //M0 
        axes[1]->controller_.input_vel_can_ = can_getSignal<int16_t>(msg, 16, 16, false);  //M1 
        
        if((abs(axes[0]->controller_.input_vel_can_) > 1500) ||
		   (abs(axes[1]->controller_.input_vel_can_) > 1500) )
        {
            axes[0]->controller_.input_vel_can_ = 0;
			axes[1]->controller_.input_vel_can_ = 0;
		}
		
        if(abs(axes[0]->controller_.input_vel_can_) < 2) 
        {
            axes[0]->controller_.input_vel_can_ = 0;
		}
		if(abs(axes[1]->controller_.input_vel_can_) < 2)
        {
            axes[1]->controller_.input_vel_can_ = 0;
		}

		axes[0]->controller_.can_com_times = 0; 
		axes[1]->controller_.can_com_times = 0;
    }
    else if((msg.buf[0] == 0xFE) && (msg.buf[1] == 0x01))  //
    {
        clear_errors_callback(axis,msg);
    }
    else if((msg.buf[0] == 0x01) && (msg.buf[1] == 0x01))  //使能调试模式
    {
        axes[0]->config_.enable_debug = msg.buf[2] ;
	    axes[1]->config_.enable_debug = msg.buf[2] ;
    }
    else if((msg.buf[0] == 0x02) && (msg.buf[1] == 0x01))  //修改编码器模式
    {
        float   kp = 0.6f,
                ki = 10.0f,
                acc =2.808616691f,
                current_band = 100.0f,
                encoder_band = 400.0f;
        //M0 controller
        axes[0]->controller_.config_.pos_gain = 0;
        axes[0]->controller_.config_.vel_gain = kp;
        axes[0]->controller_.config_.vel_integrator_gain = ki;
        axes[0]->controller_.config_.vel_ramp_rate = acc;
        //M0 motor_
        axes[0]->motor_.config_.pole_pairs = 15;
        axes[0]->motor_.config_.resistance_calib_max_voltage = 8.0f;
        axes[0]->motor_.config_.calibration_current = 4.0f;
        axes[0]->motor_.config_.requested_current_range = 16.0f;
        axes[0]->motor_.config_.torque_constant = 0.06f;
        axes[0]->motor_.config_.current_lim = 16.0f;
        axes[0]->motor_.config_.current_lim_margin = 2.0f;
        axes[0]->motor_.config_.current_control_bandwidth = current_band;
        //M0 encoder_
        axes[0]->encoder_.config_.calib_range = 0.5f;
        axes[0]->encoder_.config_.bandwidth = encoder_band;
        //axes[0]->encoder_.config_.mode = (Encoder::Mode)260;
        //M0 motor_thermistor_
        //axes[0]->motor_thermistor_.config_.gpio_pin = 4;
        //axes[0]->motor_thermistor_.config_.enabled = false;
        //M1 controller_
		axes[1]->controller_.config_.pos_gain = 0;
        axes[1]->controller_.config_.vel_gain = kp;
        axes[1]->controller_.config_.vel_integrator_gain = ki;
        axes[1]->controller_.config_.vel_ramp_rate = acc;
        axes[1]->motor_.config_.current_control_bandwidth = current_band;
        //M1 motor_
        axes[1]->motor_.config_.pole_pairs = 15;
        axes[1]->motor_.config_.resistance_calib_max_voltage = 8.0f;
        axes[1]->motor_.config_.calibration_current = 3.0f;
        axes[1]->motor_.config_.requested_current_range = 16.0f;
        axes[1]->motor_.config_.torque_constant = 0.06f;
        axes[1]->motor_.config_.current_lim = 16.0f;
        axes[1]->motor_.config_.current_lim_margin = 2.0f;
        //M1 encoder
        axes[1]->encoder_.config_.calib_range = 0.5f;
        axes[1]->encoder_.config_.cpr = 16384U;
        axes[1]->encoder_.config_.bandwidth = encoder_band;
        //axes[1]->encoder_.config_.mode = (Encoder::Mode)260;
        //M1 motor_thermistor_
        //axes[1]->motor_thermistor_.config_.gpio_pin = 3;
       /*  axes[1]->motor_thermistor_.config_.temp_limit_lower = 0;
        axes[1]->motor_thermistor_.config_.temp_limit_lower = 0; */
        //axes[1]->motor_thermistor_.config_.enabled = false;
        axes[1]->requested_state_ =  (Axis::AxisState)4;
    }
    else if((msg.buf[0] == 0x03) && (msg.buf[4] == 0x03))  //状态
    {
        //axes[0]->requested_state_ = (Axis::AxisState)msg.buf[1];
		axes[1]->requested_state_ =   (Axis::AxisState)msg.buf[5];
    }
    else if((msg.buf[0] == 0x04) && (msg.buf[1] == 0x01))  //保存
    {
        axes[0]->encoder_.config_.pre_calibrated = true;
		axes[1]->encoder_.config_.pre_calibrated = true;
        axes[0]->motor_.config_.pre_calibrated= true;
        axes[1]->motor_.config_.pre_calibrated= true;
        odrv.save_configuration();
    }
    else if((msg.buf[0] == 0x05) && (msg.buf[1] == 0x01))  //重启
    {
        odrv.reboot();
    }
}

void CANSimple::set_input_torque_callback(Axis* axis, can_Message_t& msg) {
    axis->controller_.input_torque_ = can_getSignal<float>(msg, 0, 32, true);
}

void CANSimple::set_controller_modes_callback(Axis* axis, can_Message_t& msg) {
    axis->controller_.config_.control_mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
    axis->controller_.config_.input_mode = static_cast<Controller::InputMode>(can_getSignal<int32_t>(msg, 32, 32, true));
}

void CANSimple::set_vel_limit_callback(Axis* axis, can_Message_t& msg) {
    axis->controller_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true);
}

void CANSimple::start_anticogging_callback(Axis* axis, can_Message_t& msg) {
    axis->controller_.start_anticogging_calibration();
}

void CANSimple::set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg) {
    
}

void CANSimple::set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg) {
   
}

void CANSimple::set_traj_inertia_callback(Axis* axis, can_Message_t& msg) {

}

void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        can_Message_t txmsg;
        txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
        txmsg.id += MSG_GET_IQ;
        txmsg.isExt = axis->config_.can_node_id_extended;
        txmsg.len = 8;

        uint32_t floatBytes;
        static_assert(sizeof axis->motor_.current_control_.Iq_setpoint == sizeof floatBytes);
        std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_setpoint, sizeof floatBytes);

        txmsg.buf[0] = floatBytes;
        txmsg.buf[1] = floatBytes >> 8;
        txmsg.buf[2] = floatBytes >> 16;
        txmsg.buf[3] = floatBytes >> 24;

        static_assert(sizeof floatBytes == sizeof axis->motor_.current_control_.Iq_measured);
        std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_measured, sizeof floatBytes);
        txmsg.buf[4] = floatBytes;
        txmsg.buf[5] = floatBytes >> 8;
        txmsg.buf[6] = floatBytes >> 16;
        txmsg.buf[7] = floatBytes >> 24;

        odCAN->write(txmsg);
    }
}

void CANSimple::get_vbus_voltage_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
        can_Message_t txmsg;

        txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
        txmsg.id += MSG_GET_VBUS_VOLTAGE;
        txmsg.isExt = axis->config_.can_node_id_extended;
        txmsg.len = 8;

        uint32_t floatBytes;
        static_assert(sizeof vbus_voltage == sizeof floatBytes);
        std::memcpy(&floatBytes, &vbus_voltage, sizeof floatBytes);

        // This also works in principle, but I don't have hardware to verify endianness
        // std::memcpy(&txmsg.buf[0], &vbus_voltage, sizeof vbus_voltage);

        txmsg.buf[0] = floatBytes;
        txmsg.buf[1] = floatBytes >> 8;
        txmsg.buf[2] = floatBytes >> 16;
        txmsg.buf[3] = floatBytes >> 24;

        txmsg.buf[4] = 0;
        txmsg.buf[5] = 0;
        txmsg.buf[6] = 0;
        txmsg.buf[7] = 0;

        odCAN->write(txmsg);
    }
}

void CANSimple::clear_errors_callback(Axis* axis, can_Message_t& msg) {
    axes[0]->clear_errors();
	axes[1]->clear_errors();
	
	axes[0]->flag_axis_en = true;
	axes[1]->flag_axis_en = true;
}

void CANSimple::send_heartbeat(Axis* axis) {
	    can_Message_t txmsg;
	    txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
	    txmsg.id += MSG_ODRIVE_HEARTBEAT;  // heartbeat ID
	    txmsg.isExt = axis->config_.can_node_id_extended;
	    txmsg.len = 8;

	    // Axis errors in 1st 32-bit value
	    txmsg.buf[0] = axes[0]->error_;
	    txmsg.buf[1] = axes[0]->error_ >> 8;
	    txmsg.buf[2] = axes[0]->error_ >> 16;
	    txmsg.buf[3] = axes[0]->error_ >> 24;

	    // Current state of axis in 2nd 32-bit value
	    txmsg.buf[4] = axes[1]->error_;
	    txmsg.buf[5] = axes[1]->error_ >> 8;
	    txmsg.buf[6] = axes[1]->error_ >> 16;
	    txmsg.buf[7] = axes[1]->error_ >> 24;
	    odCAN->write(txmsg);
}
void CANSimple::active_upload_error(Axis* axis) {
	    can_Message_t txmsg;
	    txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
	    txmsg.id += MSG_GET_ERROR_INFO;  
	    txmsg.isExt = 1;
	    txmsg.len = 8;

	    txmsg.buf[3] = axes[1]->motor_.error_;
	    txmsg.buf[2] = axes[1]->motor_.error_>>8;
	    txmsg.buf[1] = axes[1]->motor_.error_>>16;
	    txmsg.buf[0] = axes[1]->motor_.error_>>24;

	    txmsg.buf[7] = axes[1]->error_;
	    txmsg.buf[6] = axes[1]->error_ >> 8;
	    txmsg.buf[5] = axes[1]->error_ >> 16;
	    txmsg.buf[4] = axes[1]->error_ >> 24;
		
	    odCAN->write(txmsg);
		
}
void CANSimple::active_upload_encoder_count(Axis* axis,uint32_t times) {
	    can_Message_t txmsg;
	    txmsg.id = 0xF00000;   // 
	    txmsg.id += times;     // 
	    txmsg.isExt = 1;                      
	    txmsg.len = 8;
		
        int32_t m0_floatBytes = (int32_t)(axes[1]->motor_.current_meas_.phB*1000.0f) ;//(int32_t)(0-axes[0]->encoder_.shadow_count_); 
        txmsg.buf[7] = m0_floatBytes;
        txmsg.buf[6] = m0_floatBytes >> 8;
        txmsg.buf[5] = m0_floatBytes >> 16;
        txmsg.buf[4] = m0_floatBytes >> 24;

        int32_t m1_floatBytes = (int32_t)(axes[1]->motor_.current_meas_.phC*1000.0f) ;//(int32_t)(axes[1]->encoder_.shadow_count_); 
        txmsg.buf[3] = m1_floatBytes;
        txmsg.buf[2] = m1_floatBytes >> 8;
        txmsg.buf[1] = m1_floatBytes >> 16;
        txmsg.buf[0] = m1_floatBytes >> 24;
		
	    odCAN->write(txmsg);
		
}

void CANSimple::get_drive_error_callback(Axis* axis, can_Message_t& msg) {
    if (msg.rtr) {
	    can_Message_t txmsg;
	    txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
	    txmsg.id += MSG_GET_ERROR_INFO;  // heartbeat ID
	    txmsg.isExt = axis->config_.can_node_id_extended;
	    txmsg.len = 8;

	    // Axis errors in 1st 32-bit value
	    txmsg.buf[0] = (unsigned char)(axes[0]->error_);
	    txmsg.buf[1] = (unsigned char)(axes[0]->error_ >> 8);
	    txmsg.buf[2] = (unsigned char)(axes[0]->error_ >> 16);
	    txmsg.buf[3] = (unsigned char)(axes[0]->error_ >> 24);

	    // Current state of axis in 2nd 32-bit value
	    txmsg.buf[4] = (unsigned char)(axes[1]->error_);
	    txmsg.buf[5] = (unsigned char)(axes[1]->error_ >> 8);
	    txmsg.buf[6] = (unsigned char)(axes[1]->error_ >> 16);
	    txmsg.buf[7] = (unsigned char)(axes[1]->error_ >> 24);
	    odCAN->write(txmsg);
    }
}
void CANSimple::send_debug(Axis* axis,can_Message_t& msg) {
    if (msg.rtr) {
	    can_Message_t txmsg;
	    txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
	    txmsg.id += MSG_DEBUG;                                   //
	    txmsg.isExt = axis->config_.can_node_id_extended;
	    txmsg.len = 8;

	    // Axis errors in 1st 32-bit value
	    txmsg.buf[0] = (unsigned char)axes[0]->requested_state_ ;
	    txmsg.buf[1] = (unsigned char)axes[0]->current_state_;
	    txmsg.buf[2] = (unsigned char)axes[1]->requested_state_ ;
	    txmsg.buf[3] = (unsigned char)axes[1]->current_state_;

	    // Current state of axis in 2nd 32-bit value
	    txmsg.buf[4] = 0;
	    txmsg.buf[5] = 0;
	    txmsg.buf[6] = 0;
	    txmsg.buf[7] = 0;
	    odCAN->write(txmsg);
    }
}

uint32_t CANSimple::get_node_id(uint32_t msgID) {
    return (msgID >> NUM_CMD_ID_BITS);  // Upper 6 or more bits
}

uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
    return (msgID & 0x01F);  // Bottom 5 bits
}